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Abstract.  This  paper  describes  a  new  technique  for  tracking  locally  curved  unknown  objects 
using  sonar.  The  approach  explicitly  accounts  for  relevant  robot  dynamics.  Objects  are  tracked 
by  looking  for  temporal  sequences  of  observations  that  fit  a  kinematic  model.  The  method 
is  illustrated  using  data  from  a  synthetic  aperture  sonar  gathered  by  a  BPAUV  at  NATO 
SACLANT’s  GOATS  2002  experiment  in  the  Ligurian  Sea. 


1  Introduction  and  State-of-Art 


To  navigate  relative  to  features,  avoid  obstacles,  recognize  objects,  and  interact 
with  or  manipulate  the  underwater  environment,  substantial  improvements  in  sonar 
perception  need  to  be  made. 

Numerous  groups  have  investigated  sonar  perception.  Leonard  and  Durrant- 
Whyte  [7]  developed  regions  of  constant  depth  (RCDs)  to  determine  correspondence 
in  a  sonar  scan.  Nagatani  etal.  [10]  developed  the  Arc  Transversal  Median  method  to 
find  edges.  Mataric  and  Brooks  [2]  developed  a  robot  that  maintained  a  fixed  distance 
from  walls  by  tracking  without  an  explicit  model.  Kleeman  and  Kuc  [5]  used  the 
method  of  images  to  distinguish  corners  and  walls.  Barshan  and  Kuc  [1]  developed 
a  sonar  that  differentiated  between  convex  corners  and  walls  based  on  amplitude 
measurements.  Kuc  [6]  developed  a  sonar  that  could  recognize  objects  based  on 
waveforms;  the  sonar  adaptively  positioned  itself  to  search  for  unique  aspects  of 
the  target.  Peremans  et  al.  [13]  built  a  tri-aural  sonar  for  estimating  the  range  and 
bearing  to  a  target.  Wijk  and  Christensen  [18,19,17]  developed  Triangulation  Based 
Fusion,  a  method  for  mapping  vertical  edges  using  a  sliding  window  of  recent  sonar 
measurements.  Kleeman  [4]  designed  a  sonar  that  used  double  pulse  coding  to  reject 
interference  and  perform  classification. 

Rikoski  and  Leonard  developed  a  method  for  tracking  locally  curved  objects 
in  a  two-dimensional  environment  [15].  This  paper  will  extend  that  result  to  three 
dimensions. 
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2  Trajectory  Sonar  Perception 

Motivated  by  Mart’s  suggestion  to  "do  what  is  possible,  and  proceed  from  there 
toward  what  is  desirable"  [9],  the  perception  problem  was  broken  down  into  a 
sequence  of  processing  stages:  signal  processing  (beamforming  and  matched  filter¬ 
ing),  detection,  tracking,  modeling,  mapping,  and  object  recognition.  Each  stage  or 
competence  had  a  well  defined  input,  output,  and  transformation  and  was  heavily 
grounded  in  physics  [14].  The  approach  was  designed  to  take  advantage  of  delayed 
decision  making  [  15,8] .  Tracking,  modeling,  and  object  recognition  were  considered 
separate  problems,  as  they  happen  on  different  time  scales.  This  paper  focuses  on 
tracking. 

We  want  to  determine  the  minimum  information  necessary  to  determine  cor¬ 
respondence  between  measurements  of  static  features.  We  would  like  to  do  this 
without  knowing  what  or  where  the  feature  is.  This  is  appropriate  because  the  mod¬ 
eling  problem  is  greatly  simplified  if  correspondence  is  known. 

Trajectory  Sonar  Perception  models  geometric  relationships  between  robots  and 
features  as  potential  fields.  As  a  moving  observer  passes  through  a  field,  the  observed 
rate  of  change  of  the  field  is  known  as  the  substantial  derivative  [11].  For  a  field  <P, 
the  substantial  derivative  is  defined  as 


_d‘P  .  d<P 
Dt  dt  ^  dx 


(1) 


Given  a  prior  measurement  of  the  field  can  be  predicted  using  a  Taylor 

series  expansion  of  substantial  derivatives 
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Assume  a  sensor  that  measures  a  geometric  relationship  between  a  robot  and  a 
target.  Assuming  that  the  geometric  measurements  vary  continuously,  and  assuming 
the  robot  motion  to  be  continuous,  it  is  reasonable  to  apply  the  substantial  derivative. 
A  typical  marine  robot  displaces  50-500kg,  transmits  its  sonar  3-10  times  per  second, 
and  is  perturbed  by  waves  with  periods  on  the  order  of  5-30  seconds,  making  these 
assumptions  reasonable. 

Assume  an  initial  measurement  of  a  target  [r  6*  (/)]  .A  subsequent  measurement 
can  be  predicted  through  a  Taylor  series  expansion  of  substantial  derivatives. 


ri 

'ro+^At+^^+h.ox: 

0i 

= 

Oq  +  +  h.o.t. 
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(j)Q  +  +  h.o.t. 

The  Taylor  series  for  range  includes  a  second  order  term.  As  will  be  shown 
later,  the  second  substantial  derivative  of  range  is  equivalent  to  the  first  substantial 
derivatives  of  azimuth  and  elevation.  The  second  substantial  derivative  of  range  is 
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In  the  typical  definition  of  the  substantial  derivative,  x  is  the  vector  of  vehicle 
velocities  [tt  u  w]  .  For  the  purposes  of  this  paper,  x  will  be  extended  to  include 

the  roll,  pitch,  and  yaw  rates  of  the  robot  [P  Q  i?]^.  This  is  because  the  relative 
target  bearing  changes  as  the  robot  turns.  Expanded,  the  substantial  derivative  of 
range  becomes 


Dr 
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dr  dx 
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where  and  ^z  denote  rotation  around  the  x,  y,  and  z  axes  of  the  robot 

respectively. 

By  assuming  the  medium  to  be  coherent  [16],  ,and  ^  can  be  neglected. 

This  assumption  implies  that  when  the  robot  is  stationary  the  measurements  are 
stationary. 


3  Universal  Target  Model 

In  order  to  calculate  the  substantial  derivative  of  measurements,  one  must  have 
a  geometric  model.  In  most  prior  work,  point-like  and  planar  objects  have  been 
treated  as  distinct.  While  maintaining  discrete  feature  types  is  sound  when  there  are 
relatively  few,  this  approach  will  likely  break  down  as  more  feature  types  are  added 
and  the  differences  between  them  is  reduced.  It  would  be  desirable  to  have  a  single 
generic  feature  model  that  applies  to  all  possible  feature  types. 

By  assuming  targets  to  be  locally  curved,  it  is  possible  to  accommodate  both 
point-like  and  planar  surfaces.  If  a  locally  curved  surface  has  a  center  of  curvature 
{xc,yc,Zc)  and  a  radius  of  curvature  p,  then  a  point-like  object  has  p  =  0  and  a 
planar  surface  has  p  =  oo.  All  other  objects  have  finite  non-zero  radii  of  curvature. 
A  convex  object  has  p  >  0,  a  concave  object  has  p  <  0.  Putting  the  feature  into 
robot  coordinates  (the  robot  is  at  (0,  0, 0)  facing  in  the  x  direction),  but  leaving  the 
robot  coordinates  (x,  y,  z)  in  the  equations  to  illustrate  the  derivatives,  the  predicted 
measurement  is 


Unfortunately,  a  measurement  [r  0  ^  cannot  be  directly  inverted  to  yield 

the  target  model  [xc  t/c  Zc  p]^.  The  model  has  four  degrees  of  freedom,  but  the 
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measurement  has  only  three  degrees  of  freedom.  However,  it  is  possible  to  calculate 
the  substantial  derivatives. 

Consider  the  first  term  in  the  substantial  derivative  of  range,  The  partial 
derivative  is 


dx  ^ (xc  -  xY  +  (t/c  -  vY  +  (^c  -  zY 

Although  the  robot  does  not  know  {xc,yc,  Zc),  the  robot  can  measure  9  and  (f). 
Substituting,  it  is  evident  that 

dr 

—  = -cos(6»)sin(^).  (8) 

By  designing  the  sensor  to  measure  6  and  (j),  it  is  possible  to  essentially  measure  the 
substantial  derivative  of  range.  The  first  substantial  derivative  of  range  of  a  locally 
curved  object  calculated  using  angular  measurements  is 


—  =  —ucos{6)sm{(j))  —  sin(6l)  sin((/())  —  wcos{(j)).  (9) 

Notice  that  the  first  substantial  derivative  of  range  depends  only  on  measurements  and 
robot  velocities  (which  can  also  be  measured);  it  does  not  require  globally  referenced 
information  or  angular  rates.  Given  velocity  information,  it  can  be  calculated  entirely 
from  a  previous  measurement. 

The  derivatives  of  6  and  (j),  and  the  second  substantial  derivative  of  r  are  more 
difficult.  The  distance  to  the  center  of  curvature  is  squared  in  the 

denominator  and  cannot  be  removed  entirely  by  substituting  trigonometric  identities. 
The  range  r  cannot  be  substituted  because  equals  r  +  p,  and  p  is 

not  known.  For  instance: 

DO  usm{9)  —  vcos{9) 

Dt  sin((/))  ^{xc-xy  +  {yc  -  t/)^  +  (zc  -  z)^ 


Pcos(0)  cos((/))  +  (5sin(0)  cos((()) 
sin((/)) 

The  component  with  y/ {xc  —  x)'^  +  {yc  —  yY  +  {^c  —  z)"^  in  the  denominator 
is  the  angular  rate  of  the  target  due  to  robot  translation.  At  broadside  (0  =  |, 
(/)  =  |)  the  angular  rate  of  the  target  is  with  Tc  being  the  distance  to  the  center  of 
curvature.  Intuitively,  this  makes  sense  because  the  angular  rate  of  a  satellite  orbiting 
at  a  distance  Tc  from  the  earth’s  center  and  with  velocity  u  would  have  the  same 
angular  rate.  Unfortunately,  the  radius  of  curvature  cannot  be  observed  from  a  single 
measurement  so  this  term  cannot  be  directly  predicted. 

The  second  substantial  derivative  of  range  suffers  from  a  similar  problem.  When 

a  robot  passes  a  target  at  broadside,  the  target  appears  to  accelerate  away  from  the 
2  2 
robot  at  — .  A  satellite  orbiting  experiences  a  centripetal  acceleration  of  — ,  so  again 

c  'f'  c 
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this  is  intuitively  sensible.  Of  course,  since  the  target  is  "center  avoiding"  this  is, 
strictly  speaking,  a  centrifugal  effect. 

What  becomes  evident  is  that  by  substituting  the  first  substantial  derivatives  of 
the  angles  into  the  second  substantial  derivative  of  range,  unwanted  terms  can  be 
forced  to  drop  out.  In  addition  to  removing  the  undesired  term,  the  angular  rates 
can  be  forced  to  drop  out.  Looking  at  it  is  evident  that  if  the  robot  has  a  yaw  rate 
R,  the  angular  rate  of  the  target  will  be  —R.  If  the  robot  has  velocity  u  and  is  passing 
the  target  at  broadside  on  the  right  as  it  yaws,  it  will  accelerate  away  from  the  target 

at  uR.  If  the  robot  has  a  turning  radius  pr,  so  that  its  yaw  rate  R  =  —,  the  robot 

2 

accelerates  away  from  the  target  at  which  is  the  robot’s  centripetal  acceleration. 
Finishing  the  substitution  yields  the  second  substantial  derivative  in  range 


D^r 

1^2 


—ii  cos{6)  sin((/))  —  v  sin(0)  sin((/))  —  w  cos(^) 


(11) 


+(usin(0)  sin(^) 


vcos{9)  sin((/))) 


D0 

Ih 


—  {ucos{9)  cos{(j))  +  vsin{9)  cos(^) 


wcos{(j))) 


D(t) 

15t' 


It  is  not  possible  to  predict  the  second  substantial  derivative  in  range  from  one 
prior  measurement.  Given  two  prior  measurements,  however,  it  is  possible  to  measure 
the  angular  substantial  derivatives  and  then  predict  a  third  measurement. 


4  Non-dimensional  Analysis 


It  is  necessary  to  determine  when  terms  are  observable  for  two  reasons.  First,  it  is 
important  to  determine  how  many  terms  should  be  kept  in  the  Taylor  expansion. 
Second,  terms  such  as  the  angular  rates  must  be  observable  if  they  are  used  to 
calculate  other  quantities  such  as  the  second  substantial  derivative  of  range. 

The  first  question  is  whether  the  range  changes  enough  ping  to  ping  to  justify 
using  the  first  substantial  derivative.  It  will  be  assumed  that  the  sonar  that  has  a  ping 
rate  fs,  a  wavelength  A,  and  quarter  wavelength  resolution.  In  order  for  the  first 
substantial  derivative  of  range  to  be  observable,  the  range  must  change  more  than  a 
quarter  wavelength  between  pings: 


Dr 


At  > 


X 

4 


(12) 


Since  the  substantial  derivative  of  range  is  maximized  when  the  target  is  in  the 
robot’s  direction  of  travel,  using  V  for  the  robot’s  speed,  and  substituting  j-  for  At, 
this  can  be  reduced  to 


V 

fs 


> 


A 

4' 


(13) 
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This  leads  to  the  non-dimensional  number 
V 

Ni  =  yt-  (14) 

'^JS 

If  Ni  exceeds  j,  then  the  range  rate  is  observable.  If  the  resolution  were  some  other 
fraction  of  a  wavelength,  then  Ni  would  have  to  exceed  that  fraction.  Ni  is  also  used 
to  determine  whether  the  heave  and  sway  velocities  need  to  be  modeled.  If,  using 
the  heave  or  sway  velocity,  the  calculated  Ni  value  is  less  than  those  terms  can 
be  dropped  from  first  substantial  derivative  of  range.  This  is  important  if  velocity 
information  is  not  available,  as  it  reduces  the  minimum  number  of  features  necessary 
for  a  velocity  solution. 

The  second  substantial  derivative  of  range  is  observable  if 


At^  D^r  ^  A 

>  4' 


(15) 


It  can  be  thought  of  as  containing  three  components.  The  first  component,  described 
by  N2,  analyzes  the  tendency  of  the  range  rate  to  change  as  a  robot  drives  past  a 
target(the  previously  mentioned  centrifugal  effect).  The  second  component  considers 
the  contribution  of  the  robot’s  angular  rate  to  the  second  substantial  derivative  of 
range,  and  is  described  by  N^.  The  third  component,  the  contribution  of  acceleration, 
has  the  non-dimensional  number  N4.  The  non-dimensional  numbers  are 


N2 


1/2 


2/.^Ar 


min 


(16) 


N4 


N4 


VT 

V 


(17) 

(18) 


In  N3,  Y  represents  the  maximum  angular  rate  of  the  robot.  If  either  N2,  N^,  or  A^4 
exceed  the  fraction  describing  the  wavelength  resolution  of  the  sonar,  the  second 
substantial  derivative  of  range  is  observable. 

The  angular  rate  of  the  robot  is  due  to  two  effects.  The  target  bearing  can  change 
because  the  robot  passes  the  target,  or  because  the  robot  turns.  describes  the 
contribution  of  robot  translation,  TVg  describes  the  contribution  of  turning. 


IV5 


VL 

^  min  A/. 


(19) 


(20) 

In  both  and  Nq,  the  aperture  of  the  sonar  L  appears.  It  is  assumed  that  the  angular 
rate  of  the  target  is  observable  only  if  the  change  in  the  target  bearing  exceeds  the 
resolution  of  the  sonar.  The  broadside  resolution  of  a  line  array,  ^  is  used  for 
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simplicity.  If  a  robot  has  a  sonar  with  an  aperture  L,  the  angular  rate  is  observable  if 
TVs  or  Nq  exceed  ^ . 

The  first  six  numbers  describe  robot  effects  in  calm  water.  However,  the  ocean 
does  not  stand  still.  The  effect  of  a  current  can  be  modeled  through  a  straightforward 
application  of  Ni,  but  the  effects  of  waves  are  slightly  more  complicated.  Only 
linear  waves  will  be  considered  in  this  paper.  To  avoid  confusion  with  acoustic 
parameters,  the  parameters  describing  surface  waves  will  use  uppercase  variables 
(because  surface  waves  are  "bigger"  than  acoustic  waves).  The  relevant  parameters 
are  the  angular  frequency  Q,  amplitude  A,  wave  number  K,  and  wavelength  A. 

In  deep  water,  it  is  obvious  that  particles  on  the  water  surface  move  up  and  down 
as  waves  pass.  Particles  also  have  horizontal  motions  that  are  equal  in  in  amplitude 
to  the  vertical  motions,  but  90°  out  of  phase.  Consequently,  the  particle  motions  are 
circular,  and  the  paths  are  called  particle  orbits.  The  diameter  of  these  orbits  decay 
exponentially  with  depth.  If  a  robot  is  neutrally  buoyant  and  small  with  respect  to 
the  wavelength  of  the  wave,  it  is  reasonable  to  expect  it  to  orbit  like  a  particle.  The 
non-dimensional  number  used  to  describe  linear  deep  water  waves  is 


A^7 


/.A 


(21) 


The  z  variable  is  referenced  to  the  surface  and  underwater  values  are  negative.  At  a 
depth  of  half  a  wavelength  A,  the  amplitude  of  a  particle  orbit  is  reduced  to  Ae~'^ . 
However,  a  moderate  ocean  wave  can  have  a  wavelength  in  excess  of  100m,  so  wave 
effects  can  extend  considerably  below  the  surface. 

The  shallow  water  case  is  more  complicated.  The  vertical  motion  of  water 
particles  is  restricted,  as  they  cannot  pass  into  the  sea  floor.  Shallow  water  wave 
particles  move  in  ellipses;  at  the  bottom  the  particles  only  move  horizontally.  The 
non-dimensional  number  that  describes  the  contribution  of  linear  shallow  water 
waves  is 


gAKcoA^jK^zAh)) 

®  fl  fsXcosh{Kh) 

For  this  number,  h  is  the  water  depth  and  g  =  9.8^  (strictly  speaking  ocean  waves 
are  called  gravity  waves,  because  gravity  is  the  restoring  force). 


5  Experimental  Setup 

In  2002,  in  conjunction  with  the  NATO  SACLANT  research  center  in  La  Spezia, 
Italy,  a  set  of  missions  were  conducted  as  part  of  the  GOATS  2002  experiment.  The 
experiment  was  conducted  using  Caribou,  an  Odyssey  III  vehicle  built  by  Bluefin 
Robotics.  The  Odyssey  III  is  a  streamlined  vehicle  with  a  vectored  thruster.  The 
vehicle  featured  a  16-element  dual  line  array  synthetic  aperture  sonar  built  by  MIT 
doctoral  candidates  Don  Eicksted  and  T.  C.  Liu,  and  post-doctoral  fellow  Wen  Xu. 
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Since  the  drag  on  the  cylindrical  acoustic  transmitter  likely  exceeded  the  vehicle 
drag  by  an  order  of  magnitude,  it  was  hidden  inside  the  vehicle  hull,  which  was 
designed  to  be  acoustically  transparent.  The  transmitter  was  aimed  at  broadside,  and 
transmitted  a  5  —  25kHz  linear  chirp.  The  receiver,  mounted  on  the  nose,  consisted 
of  two  parallel  arrays  of  eight  elements.  The  elements  were  spaced  10cm  apart  and 
the  arrays  were  spaced  15cm  apart. 


Fig.  1.  On  the  left,  the  robot.  Caribou,  being  lowered  into  the  water  off  the  coast  of  Elba.  On 
the  right  is  a  close  up  of  the  16-element  dual  line  array  used  to  receive  the  sonar  signals.  The 
green  spheres  are  individual  hydrophones. 


A  characteristic  along-track  robot  velocity  was  1.5  —  ,  a  representative  wave¬ 
length  was  0.1m,  and  the  ping  rate  fs  —  3Hz.  Since  iVi  =  5,  the  range  rate  was 
observable.  Although  direct  sea  state  measurements  were  unavailable,  wave  induced 
heave  and  sway  velocities  between  0.1  —  and  0.25—  were  routinely  observed.  Since 
any  velocity  in  excess  of  0.075—  would  cause  noticeable  contribute  to  the  range 
rate,  the  contributions  of  the  induced  heave  and  sway  velocities  were  observable. 

The  robot  typically  operated  10m  off  the  bottom.  At  that  minimum  range,  N2  = 
0.125.  The  maximum  yaw  rate  and  acceleration  were  approximately  T  =  = 

0.1745^  and  O.ip^  respectively.  The  other  two  measures  of  the  second  substantial 
derivative  of  range  were  A3  =  0.22  and  A^4  =  0.06.  Although  A3  put  the  robot 
close  to  the  transition,  the  second  substantial  derivative  was  neglected.  Most  targets 
were  at  least  20m  away,  and  targets  directly  under  the  robot  were  usually  masked  by 
the  powerful  bottom  reflection. 

Since  =  0.35  and  Nq  =  0.41,  the  angular  rates  were  barely  observable. 
The  beamformer  used  to  produce  the  results  in  [12]  was  noisy  enough  to  justify 
ignoring  the  angular  rate.  The  beamformer  used  in  this  paper  allowed  the  angular 
rate  to  occasionally  be  observable.  Nevertheless,  the  information  was  not  used. 
Consequently,  the  measurement  prediction  used  in  this  paper  was 


ri 

ro  —  {ucos{9)  sin(0)  +  vsm{9)  sin(0)  +  wcos{(j)))At 

0i 

= 

Oo 

4>o 

(23) 
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The  results  presented  here  are  from  the  Caribou  mission  initiated  at  4: 1 1pm  on 
June  15, 2002,  off  the  coast  of  Framura,  Italy.  In  the  GOATS  data  sets,  this  mission  is 

referred  to  as  SAS_15_6J2002 _ 16_11.  This  is  the  same  data  set  used  in  [12].  The 

mission  lasted  for  roughly  half  an  hour,  and  was  conducted  in  approximately  20m 
of  water  about  500m  from  shore.  The  robot  started  on  the  surface,  dove  to  roughly 
10m,  and  then  circled  the  targets.  The  vehicle  path  is  shown  in  Fig  2. 


Fig.  2.  The  robot  path  is  shown  on  the  left.  Since  the  sonar  transmitted  out  the  left  side  of  the 
vehicle,  the  vehicle  constantly  turned  left  to  keep  the  target  field  insonified.  A  typical  matched 
filtered  waveform  is  shown  on  the  right.  The  pulse  at  the  front  of  the  waveform  is  a  remnant 
of  the  outgoing  signal.  The  next  three  pulses  correspond  to  the  reflection  off  the  surface,  the 
reflection  off  the  bottom,  and  the  multipath  reflection  off  the  surface  and  then  the  bottom. 
Potential  targets  are  evident  in  the  second  half  of  the  waveform. 


6  Experimental  Results 

Starting  with  raw  sonar  signals,  the  robot  systematically  refined  the  data,  building 
towards  higher  level  constructs.  The  data  was  matched  filtered,  beamformed,  and 
passed  to  a  detector.  The  output  of  the  detector  was  passed  to  feature  tracker, 
which  determined  correspondence  between  pings  of  unknown  objects  using  vehicle 
dynamics.  The  time  series  of  raw  detections  and  tracked  features  are  shown  in  Fig  3 
and  Fig  4.  Detections  are  shown  in  blue  and  measurements  from  tracked  targets  are 
shown  in  red. 

In  the  time  series  of  measurements,  the  detections  at  3  meters  are  artifacts  of  the 
outgoing  chirp.  The  two  curves  that  are  centered  at  approximately  10  meters  are  the 
reflections  off  the  ocean  surface  and  bottom.  The  two  curves  are  mirror  images  of 
one  another  because,  for  a  flat  bottom,  if  the  robot  depth  increases  the  robot  altitude 
decreases.  The  measurements  of  the  surface  reflections  appear  to  be  noisier  than 
measurements  of  the  bottom  because  the  ocean  surface  is  not  static. 

The  slowly  varying  line  at  20m  corresponds  to  the  water  depth.  The  signal  reflects 
off  the  surface,  off  the  bottom,  and  then  back  to  the  robot  (or  off  the  bottom,  then 


10 


R.  Rikoski  and  J.  Leonard 


adjusted  llfnesleps  adjusted  timesteps 


Fig.  3.  This  figure  shows  measurements  plotted  in  measurement  space.  The  x-axis  is  adjusted 
timestep,  the  y-axis  is  range  in  meters.  Detections  are  plotted  in  blue,  measurement  trajectories 
are  shown  in  red.  The  measurements  on  the  right  are  for  pings  1000  to  2000  (adjusted  timesteps 
1 167  to  2334),  the  measurements  on  the  right  are  for  pings  2000  to  3000. 


Fig.  4.  On  the  left,  the  measurements  and  tracked  features  for  pings  3000  to  4000.  Pings  4000 
to  5000  are  shown  on  the  right. 


off  the  surface,  then  back  to  the  robot,  there  are  two  paths).  Centered  around  30tn 
are  multi-path  observations  of  the  water  surface  and  bottom.  The  surface  multi-path 
reflection  would  bounce  off  the  surface,  off  the  bottom,  off  the  surface,  and  then 
back  to  the  robot.  The  reflections  at  approximately  40m  are  at  twice  the  water  depth, 
and  correspond  to  two  reflections  off  the  surface  and  bottom.  Each  reflection  off  the 
surface  introduces  unmodeled  noise  into  the  sequence  of  measurements,  since  the 
water  surface  changes  shape  from  ping  to  ping.  Consequently,  multi-path  reflections 
are  not  tracked  well. 

The  short  vertical  streaks  are  due  to  signals  from  the  acoustic  transponders  used 
for  navigation.  The  long  wide  streaks  correspond  to  acoustic  modem  transmissions. 
Both  signals  tended  to  cause  numerous  false  alarms  at  the  level  of  the  detector. 

The  detections  and  measurements  from  tracked  features  from  pings  3000  to  4000 
are  projected  into  global  coordinates  in  Fig  5.  Since  the  altitude  and  depth  reflections 
are  from  directly  below  and  above  the  robot,  they  correspond  to  the  vehicle’s  path. 
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Notice  that,  in  general,  this  is  an  awkward  representation  for  feature  extraction.  Also 
in  Fig  5,  there  is  a  sequence  of  measurements  corresponding  to  a  mid-water  target. 
This  target  was  not  tracked.  This  was  likely  due  to  one  of  two  causes.  First,  it  was 
inside  the  nearfield  of  the  array.  Bearing  is  a  well  defined  construct  for  targets  in  the 
farheld  of  an  array.  However,  when  the  farheld  definition  is  applied  to  targets  in  the 
nearfield,  the  results  range  from  imprecise  to  nonsensical. 

Tracked  targets  start  to  appear  after  the  bottom  bounce,  and  extend  out  to  75m. 
Typical  sequences  of  target  measurements  and  predictions  are  shown  in  Figs  6  and 
8.  The  robot  is  able  to  predict  measurements  to  a  fraction  of  wavelength.  Differences 
between  the  predicted  and  actual  measurements  are  primarily  due  to  navigational 
errors.  This  is  especially  evident  when  comparing  the  predicted  and  actual  range 
rates  (Figs  7  and  9).  The  predicted  range  rate  tends  to  lag  the  actual  range  rate.  This 
is  most  likely  because  the  state  estimate  from  the  navigation  filter  lags  the  true  robot 
state. 
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Fig.  5.  On  the  right,  the  detections  and  tracked  features  from  pings  3000  to  4000  are  projected 
into  cartesian  coordinates.  Clearly,  this  representation  has  less  structure,  making  feature 
extraction  more  difficult.  On  the  right  is  a  feature  that  the  algorithm  failed  to  track.  The  curve 
on  the  right  corresponds  to  a  midwater  object.  Although  the  measurement  are  continuous, 
the  measurements  violate  two  assumptions.  First,  the  second  substantial  derivative  of  range 
is  non-negligible.  Second,  because  the  target  is  so  close  it’s  inside  the  nearfield  of  the  array. 
The  sequence  of  measurements  at  2.5  and  3  meters  are  artifacts  from  the  transmitted  signal. 


7  Conclusion 

Using  the  constraint  based  on  a  universal  feature  model,  a  variety  of  features  (point 
objects,  concrete  blocks,  simple  geometric  shapes,  the  surface,  the  ocean  floor) 
were  tracked  in  a  dynamic  shallow  water  environment.  The  technique  was  able  to 
find  regularities  in  the  data  despite  measurement  and  navigational  uncertainty,  and 
numerous  interfering  sources  such  as  acoustic  modems  and  long  baseline  navigation 
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Fig.  6.  On  the  right,  a  time  series  of  measurements  from  a  single  target.  The  x-axis  is  adjusted 
timestep,  the  y  axis  is  range.  The  measurements  are  plotted  as  red  points  connected  by  the 
black  line,  the  predicted  measurements  are  plotted  in  blue.  The  graph  on  the  right  shows  the 
difference  between  the  predicted  and  actual  measurements.  The  errors  are  on  the  order  of 
centimeters  despite  the  robot  moving  at  roughly  1.5^  and  the  target  being  over  50m  away. 


Fig.  7.  The  predicted  and  measured  substantial  derivatives  of  range  are  shown  on  the  left.  The 
range  rate  calculated  from  measurements  is  shown  as  red  points  connected  by  a  black  line, 
the  range  rate  predicted  using  vehicle  dynamics  is  shown  as  black  points  connected  by  a  blue 
line.  The  second  substantial  derivative  of  range  as  calculated  from  sets  of  three  measurements 
is  shown  on  the  right.  Using  non-dimensional  analysis  it  was  concluded  that  this  derivative 
would  not  noticeably  affect  the  time  sequence  of  measurements  from  ping  to  ping.  The  data 
agrees  with  the  non-dimensional  analysis;  the  second  substantial  derivative  is  essentially  zero 
mean  noise. 


beacons.  The  processed  measurements  were  used  by  Newman  as  the  input  to  a 
constant  time  SLAM  algorithm  [12]. 

There  are  four  obvious  research  paths  leading  from  this  work.  First,  the  work 
should  be  extended  to  dynamic  objects.  Second,  techniques  for  creating  geometric 
models  of  features  given  correspondence  need  to  be  investigated.  Third,  the  approach 
needs  to  be  extended  beyond  the  Freedman  model  [3]  to  include  rough  surface 
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Fig.  8.  A  second  time  series  of  measurements  from  a  target,  and  the  difference  between  the 
predicted  and  observed  range.  In  this  case,  the  robot  is  turning  while  tracking  the  feature.  This 
sequence  of  measurements  has  greater  range  errors,  due  to  velocity  errors  as  the  robot  turns. 


Fig.  9.  Looking  at  the  range  rates,  it  is  apparent  why  there  were  range  errors  in  the  previous 
figure.  The  navigation  filter  tends  to  lag  the  hue  state,  that  lag  causes  errors  in  the  predicted 
range  rate,  which  causes  errors  in  the  range  prediction. 


scattering.  Finally,  this  analysis  needs  to  be  used  to  create  a  holistic  design  guide  for 
marine  robots  using  sonar. 
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